#include "Inf_QMI8658A.h"
#include "Com_Debug.h"
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "math.h"

#define TAG "QMI8658A"

/**
 * @brief 初始化I2C
 *
 * @return esp_err_t
 */
esp_err_t bsp_i2c_init(void) {
  i2c_config_t conf = {
      // 模式 主模式
      .mode = I2C_MODE_MASTER,
      // 数据线
      .sda_io_num = I2C_MASTER_SDA_IO,
      // 时钟线
      .scl_io_num = I2C_MASTER_SCL_IO,
      // 时钟线 默认上拉
      .sda_pullup_en = GPIO_PULLUP_ENABLE,
      // 数据线 默认上拉
      .scl_pullup_en = GPIO_PULLUP_ENABLE,
      .master.clk_speed = I2C_MASTER_FREQ_HZ,
  };
  // 添加配置
  i2c_param_config(I2C_NUM_0, &conf);

  // 安装
  return i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);
}

/**
 * @brief 初始化
 *
 */
void Inf_QMI8658A_Init(void) {

  // 初始化 I2C
  esp_err_t err = bsp_i2c_init();
  if (err != ESP_OK) {
    MY_LOGE("初始化I2C失败");
  }

  // 读取芯片信息
  uint8_t is_next = 5;
  Com_Status status;
  {
    if (is_next-- == 0) {
      //  MY_LOGE("读取芯片数据错误");
      ESP_LOGE(TAG, "读取芯片数据错误");
      return;
    }
    status = Inf_QMI8658A_ChipInfo();
    vTaskDelay(50);
  }
  while (status != Com_OK) {
  };
  MY_LOGE("读取芯片数据成功");
  // 设置陀螺仪默认值
  Inf_QMI8658A_register_write_byte(QMI8658_RESET, 0xb0); // 复位
  vTaskDelay(10 / Inf_portTICK_PERIOD_MS);               // 延时10ms
  Inf_QMI8658A_register_write_byte(QMI8658_CTRL1, 0x40); // CTRL1 设置地址自动增加
  Inf_QMI8658A_register_write_byte(QMI8658_CTRL7, 0x03); // CTRL7 允许加速度和陀螺仪
  Inf_QMI8658A_register_write_byte(QMI8658_CTRL2, 0x95); // CTRL2 设置ACC 4g 250Hz
  Inf_QMI8658A_register_write_byte(QMI8658_CTRL3, 0xd5); // CTRL3 设置GRY 512dps 250Hz
}

/**
 * @brief 读取加速度和陀螺仪
 *
 * @param p 倾角结构体 指针
 */
void qmi8658_Read_AccAndGry(t_sQMI8658 *p) {

  // 读取数据
  uint8_t status, data_ready = 0;
  // 获取数据
  uint8_t buf[12];

  // 读取数据
  Inf_QMI8658A_register_read(QMI8658_STATUS0, &status, 1);
  // // 判断加速度和陀螺仪数据是否可读
  if ((status & 0x03) > 0) {
    data_ready = 1;
  }
  if (data_ready) {
    // AngleX: 7.888774, AngleY: -8.315445, AngleZ: 11.500867
    // 获取对应的数据 12 = 6 * 2, 12个 uint8_t
    Inf_QMI8658A_register_read(QMI8658_AX_L, (uint8_t *)buf, 12);
    p->acc_x = (buf[1] << 8) | (buf[0] & 0xff);
    p->acc_y = (buf[3] << 8) | (buf[2] & 0xff);
    p->acc_z = (buf[5] << 8) | (buf[4] & 0xff);
    p->gyr_x = (buf[7] << 8) | (buf[6] & 0xff);
    p->gyr_y = (buf[9] << 8) | (buf[8] & 0xff);
    p->gyr_z = (buf[11] << 8) | (buf[10] & 0xff);
    data_ready = 0;
  }
}

// 获取XYZ轴的倾角值
void qmi8658_fetch_angleFromAcc(t_sQMI8658 *p) {
  float temp;

  qmi8658_Read_AccAndGry(p); // 读取加速度和陀螺仪的寄存器值
  // 根据寄存器值 计算倾角值 并把弧度转换成角度
  temp = (float)p->acc_x / sqrt(((float)p->acc_y * (float)p->acc_y + (float)p->acc_z * (float)p->acc_z));
  p->AngleX = atan(temp) * 57.29578f; // 180/π=57.29578
  temp = (float)p->acc_y / sqrt(((float)p->acc_x * (float)p->acc_x + (float)p->acc_z * (float)p->acc_z));
  p->AngleY = atan(temp) * 57.29578f; // 180/π=57.29578
  temp = sqrt(((float)p->acc_x * (float)p->acc_x + (float)p->acc_y * (float)p->acc_y)) / (float)p->acc_z;
  p->AngleZ = atan(temp) * 57.29578f; // 180/π=57.29578
}

/**
 * @brief 读取数据
 *
 * @param reg_addr 数据地址
 * @param data 数据
 * @param len 数据长度
 * @return esp_err_t
 */
esp_err_t Inf_QMI8658A_register_read(uint8_t reg_addr, uint8_t *data, size_t len) {
  return i2c_master_write_read_device(I2C_NUM_0, QMI8658_SENSOR_ADDR, &reg_addr, 1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

/**
 * @brief 写入数据
 *
 * @param reg_addr 寄存器地址
 * @param data 数据
 * @return esp_err_t
 */
esp_err_t Inf_QMI8658A_register_write_byte(uint8_t reg_addr, uint8_t data) {

  uint8_t write_buf[2] = {reg_addr, data};
  return i2c_master_write_to_device(I2C_NUM_0, QMI8658_SENSOR_ADDR, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

/**
 * @brief 读取芯片基础信息
 *
 */
Com_Status Inf_QMI8658A_ChipInfo(void) {
  // 读取数据
  uint8_t id;
  Inf_QMI8658A_register_read(0x00, &id, 1);
  if (id == 0X05) {
    return Com_OK;
  }
  return Com_ERROR;
}
